﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Viewer3D
{
    class Camera
    {
        /// <summary>
        /// 内参数
        /// </summary>
        private double fx;
        private double fy;
        private double cx;
        private double cy;

        /// <summary>
        /// 外参数
        /// </summary>
        private Rotation3 R;
        private Point3  t;

        //public Camera()
        //{
        //    R = new Rotation3();
        //    t = new Point3();
        //}

        public Camera(double fx, double fy, double cx, double cy)
        {
            this.fx = fx;
            this.fy = fy;
            this.cx = cx;
            this.cy = cy;
            R = new Rotation3();
            t = new Point3();
        }

        public Camera(double fx, double fy, double cx, double cy, Point3 axe, double angle_deg)
        {
            this.fx = fx;
            this.fy = fy;
            this.cx = cx;
            this.cy = cy;
            R = new Rotation3(axe, angle_deg);
            t = new Point3();
        }

        public Camera(double fx, double fy, double cx, double cy, Point3 t)
        {
            this.fx = fx;
            this.fy = fy;
            this.cx = cx;
            this.cy = cy;
            R = new Rotation3();
            t = new Point3(t.X, t.Y, t.Z);
        }

        public Point2 Project(Point3 X)
        {
            Point3 P_camera = new Point3();
            P_camera = R * X + t;
            double x, y;
            x = (fx * P_camera.X) / P_camera.Z + cx;
            y = (fy * P_camera.Y) / P_camera.Z + cy;
            return new Point2(x, y);
        }

        public Rotation3 Rotation
        {
            get { return R; }
            set { R = value; }
        }

        public Point3 Translation
        {
            get { return t; }
        }

        public double FoculLength
        {
            get { return fx; }
            set { fx = fy = value; }
        }

        public void SetImageSize(int width, int height)
        {
            cx = width / 2;
            cy = height / 2;
        }

    }

    class Point2
    {
        double x, y;

        public Point2()
        {
            x = y = 0;
        }
        public Point2(double x, double y)
        {
            this.x = x;
            this.y = y;
        }
        public double X
        {
            get { return x; }
            set { x = value; }
        }
        public double Y
        {
            get { return y; }
            set { y = value; }
        }
        public double Norm()
        {
            return Math.Sqrt(x * x + y * y);
        }
        public Point2 Unitary()
        {
            double norm = Norm();
            if(norm < 1e-4)
            {
                return this;
            }
            else
            {
                return new Point2(x / norm, y / norm);
            }
        }
        public Point2 Rotate(double angle_deg)
        {
            double angle_rad = angle_deg / 180 * Math.PI;
            double c1 = Math.Cos(angle_rad), c2 = Math.Sin(angle_rad);
            double x_new = c1 * x - c2 * y;
            double y_new = c2 * x + c1 * y;
            return new Point2(x_new, y_new);
        }
        public static Point2 operator -(Point2 X1, Point2 X2)
        {
            Point2 diff = new Point2(0,0);
            diff.x = X1.x - X2.x;
            diff.y = X1.y - X2.y;
            return diff;
        }
        public static Point2 operator +(Point2 X1, Point2 X2)
        {
            Point2 sum = new Point2(0, 0);
            sum.x = X1.x + X2.x;
            sum.y = X1.y + X2.y;
            return sum;
        }
    }

    class Point3
    {
        double x, y, z;

        public Point3()
        {
            x = y = z = 0;
        }
        public Point3(double x, double y, double z)
        {
            this.x = x;
            this.y = y;
            this.z = z;
        }

        public void Copy(Point3 P)
        {
            x = P.x;
            y = P.y;
            z = P.z;
        }

        public static Point3 operator +(Point3 X, Point3 Y)
        {
            Point3 sum = new Point3();
            sum.x = X.x + Y.x;
            sum.y = X.y + Y.y;
            sum.z = X.z + Y.z;
            return sum;
        }
        public double X
        {
            get { return x; }
            set { x = value; }
        }
        public double Y
        {
            get { return y; }
            set { y = value; }
        }
        public double Z
        {
            get { return z; }
            set { z = value; }
        }
    }

    class Matrix3
    {
        protected double[,] a = new double[3,3];
        public Matrix3()
        {
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j<3; j++)
                {
                   a[i, j] = 0;
                }                   
            }
        }       

        public Matrix3(Point3 P)
        {
            a[0, 0] = a[1, 1] = a[2, 2];
            a[0, 1] = -P.Z;
            a[0, 2] =  P.Y;
            a[1, 0] = P.Z;
            a[1, 2] = -P.X;
            a[2, 0] = -P.Y;
            a[2, 1] = P.X;
        }

        public Matrix3(Matrix3 M)
        {
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    a[i, j] = M.a[i, j];
                }
            }
        }

        public void Copy(Matrix3 M)
        {
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    a[i, j] = M.a[i, j];
                }
            }
        }

        public static Matrix3 Eye()
        {
            Matrix3 I = new Matrix3();
            I.a[0,0] = I.a[1,1] = I.a[2,2] = 1;
            return I;
        }

        public static Matrix3 operator +(Matrix3 A, Matrix3 B)
        {
            Matrix3 sum = new Matrix3();
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    sum.a[i, j] = A.a[i,j] + B.a[i,j];
                }
            }
            return sum;
        }
        public static Matrix3 operator *(Matrix3 A, Matrix3 B)
        {
            Matrix3 prod = new Matrix3();
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    for (int k = 0; k < 3; k++)
                    {
                        prod.a[i,j] += A.a[i, k] * B.a[k, j];
                    }
                }
            }
            return prod;
        }
        public static Matrix3 operator *(double m, Matrix3 A)
        {
            Matrix3 prod = new Matrix3();
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    prod.a[i, j] = A.a[i, j] * m;
                }
            }
            return prod;
        }

        public static Point3 operator *(Matrix3 A, Point3 X)
        {
            Point3 Y = new Point3();
            Y.X = A.a[0, 0] * X.X + A.a[0, 1] * X.Y + A.a[0, 2] * X.Z;
            Y.Y = A.a[1, 0] * X.X + A.a[1, 1] * X.Y + A.a[1, 2] * X.Z;
            Y.Z = A.a[2, 0] * X.X + A.a[2, 1] * X.Y + A.a[2, 2] * X.Z;
            return Y;
        }
    }

    class Rotation3 : Matrix3
    {
        public Rotation3()
        {
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    a[i, j] = 0;
                }
            }
            a[0, 0] = a[1, 1] = a[2, 2] = 1;
        }

        public Rotation3(Point3 axe, double angle_deg)
        {
            double angle_rad = angle_deg / 180 * Math.PI;
            Matrix3 R = new Matrix3();
            Matrix3 n_x = new Matrix3(axe);
            Matrix3 I = new Rotation3();
            R = I + Math.Sin(angle_rad) * n_x + (1 - Math.Cos(angle_rad)) * n_x * n_x;
            Copy(R);
        }

        public static Rotation3 operator *(Rotation3 R1, Rotation3 R2)
        {
            Rotation3 prod = new Rotation3();
            prod.a[0, 0] = prod.a[1, 1] = prod.a[2, 2] = 0;
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    for (int k = 0; k < 3; k++)
                    {
                       prod.a[i, j] += R1.a[i, k] * R2.a[k, j];
                    }
                }
            }
            return prod;
        }
    }
}
